#include "arm_controller_impl.h"
using namespace std;
using namespace Eigen;
#include "communication/typetransfer.h"
#include <predef.h>

ArmControllerImpl::ArmControllerImpl(const u_char index, const int joint_num):joint_num_(joint_num),target_index_(index)
{
    arm_end_pos_target_=Vector3d::Zero();
    arm_end_quat_target_=Eigen::Quaterniond::Identity();
    arm_end_v_target_=Eigen::Vector3d::Zero();
    arm_end_w_target_=Eigen::Vector3d::Zero();

    finger_joint_pos_target_=0;
    finger_joint_vel_target_=0;
    arm_joint_pos_target_=VectorXd::Zero(joint_num_);
    arm_joint_vel_target_=VectorXd::Zero(joint_num_);

    arm_end_pos_cur_=Eigen::Vector3d::Zero();
    //TO DELETE
    arm_end_pos_cur_<<0,-0.25,-0.4;
    //
    arm_end_quat_cur_=Quaterniond::Identity();
    arm_end_v_cur_=Eigen::Vector3d::Zero();
    arm_end_w_cur_=Eigen::Vector3d::Zero();
    arm_end_wrench_cur_=VectorXd::Zero(6);

    arm_joint_pos_cur_=VectorXd::Zero(joint_num_);
    arm_joint_vel_cur_=VectorXd::Zero(joint_num_);
    arm_joint_force_cur_=VectorXd::Zero(joint_num_);

    finger_joint_pos_cur_=0;
    finger_joint_vel_cur_=0;
    finger_joint_force_cur_=0;
}

ArmControllerImpl::~ArmControllerImpl()
{

}

/*
 *  const int joint_num_;
 *  Eigen::VectorXd arm_joint_pos_cur_,arm_joint_vel_cur_;
 *  Eigen::VectorXd arm_end_wrench_cur_;
 *  double finger_joint_pos_cur_,finger_joint_vel_cur_,finger_joint_force_cur_;
 */
bool ArmControllerImpl::ParseUserData(const u_char *buffer, const size_t buffer_len)
{
    if(buffer_len<=2)
        return false;
    size_t index=0;
//    u_char part=0;
//    TypeTransfer<u_char>::ByteArray2Type(part,buffer+index);
    index+=sizeof (u_char);
    u_char attr=0;
    TypeTransfer<u_char>::ByteArray2Type(attr,buffer+index);
    index+=sizeof (attr);
    SocketDataType data;
    //enum ArmAttrType{kEndPos=0x01,kEndQuat=0x02,kEndLinearVel=0x04,kEndAngularVel=0x08,kEndForce=0x10,kJointPos=0x20,kJointVel=0x40,kJointForce=0x80};
    size_t total_len=(attr&Slave2Master::ArmAttrType::kEndPos)?3:0
                     +(attr&Slave2Master::ArmAttrType::kEndQuat)?4:0
                     +(attr&Slave2Master::ArmAttrType::kEndLinearVel)?3:0
                     +(attr&Slave2Master::ArmAttrType::kEndAngularVel)?3:0
                     +(attr&Slave2Master::ArmAttrType::kEndForce)?6:0
                     +(attr&Slave2Master::ArmAttrType::kJointPos)?joint_num_:0
                     +(attr&Slave2Master::ArmAttrType::kJointVel)?joint_num_:0
                     +(attr&Slave2Master::ArmAttrType::kJointForce)?joint_num_:0;
    total_len=total_len*sizeof (SocketDataType);
    if(buffer_len!=(1+1+total_len+2))//part+attr+data+crc
        return false;
    if(attr&Slave2Master::ArmAttrType::kEndPos)
    {
        cout<<"end pos updated"<<endl;
        for (int i=0;i<3;i++) {
            TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
            arm_end_pos_cur_[i]=static_cast<double>(data);
            index+=sizeof (data);
        }
    }
    if(attr&Slave2Master::ArmAttrType::kEndQuat)
    {
        cout<<"end quat updated"<<endl;
        TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
        arm_end_quat_cur_.w()=static_cast<double>(data);
        index+=sizeof (data);
        TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
        arm_end_quat_cur_.x()=static_cast<double>(data);
        index+=sizeof (data);
        TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
        arm_end_quat_cur_.y()=static_cast<double>(data);
        index+=sizeof (data);
        TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
        arm_end_quat_cur_.z()=static_cast<double>(data);
        index+=sizeof (data);
    }
    if(attr&Slave2Master::ArmAttrType::kEndLinearVel)
    {
        for (int i=0;i<3;i++) {
            TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
            arm_end_v_cur_[i]=static_cast<double>(data);
            index+=sizeof (data);
        }
    }
    if(attr&Slave2Master::ArmAttrType::kEndAngularVel)
    {
        for (int i=0;i<3;i++) {
            TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
            arm_end_w_cur_[i]=static_cast<double>(data);
            index+=sizeof (data);
        }
    }
    if(attr&Slave2Master::ArmAttrType::kEndForce)
    {
        for (int i=0;i<6;i++) {
            TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
            arm_end_wrench_cur_[i]=static_cast<double>(data);
            index+=sizeof (data);
        }
    }
    if(attr&Slave2Master::ArmAttrType::kJointPos)
    {
        for (int i=0;i<joint_num_;i++) {
            TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
            arm_joint_pos_cur_[i]=static_cast<double>(data);
            index+=sizeof (data);
        }
    }
    if(attr&Slave2Master::ArmAttrType::kJointVel)
    {
        for (int i=0;i<joint_num_;i++) {
            TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
            arm_joint_vel_cur_[i]=static_cast<double>(data);
            index+=sizeof (data);
        }
    }
    if(attr&Slave2Master::ArmAttrType::kJointForce)
    {
        for (int i=0;i<joint_num_;i++) {
            TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
            arm_joint_force_cur_[i]=static_cast<double>(data);
            index+=sizeof (data);
        }
    }

    return true;
}


/*
 *  Eigen::Vector3d arm_end_pos_target_;
 *  Eigen::Quaterniond arm_end_quat_target_;
 *  Eigen::Vector3d arm_end_v_target_;
 *  Eigen::Vector3d arm_end_w_target_;
 *
 *
 *  double finger_joint_pos_target_;
 */

//enum ArmAttrType{kEndPos=0x01,kEndQuat=0x02,kEndLinearVel=0x04,kEndAngularVel=0x08,kGripperPos=0x10,kGripperVel=0x20,kJointPos=0x40,kJointVel=0x80};
bool ArmControllerImpl::PackUserData(u_char *buffer,size_t &len)
{
    size_t index=0;
    if(updated_attr_==0x00)
    {
        len=0;
        return false;
    }
    //u_char type=GetType();
    TypeTransfer<u_char>::Type2ByteArray(buffer+index,target_index_);
    index+=sizeof (target_index_);
    TypeTransfer<u_char>::Type2ByteArray(buffer+index,updated_attr_);
    index+=sizeof (updated_attr_);

    if (updated_attr_&Master2Slave::kEndPos)
    {
#ifdef PRINT_LOG
        cout<<"target_end_pos: ";
#endif
        for (int i=0;i<3;i++)
        {
            TypeTransfer<SocketDataType>::Type2ByteArray(buffer+index,static_cast<SocketDataType>(arm_end_pos_target_[i]));
            index+=sizeof (SocketDataType);
#ifdef PRINT_LOG
            cout<<arm_end_pos_target_[i]<<","
#endif
        }
    }
    if (updated_attr_&Master2Slave::kEndQuat)
    {
#ifdef PRINT_LOG
        cout<<" target_end_quat: "<<arm_end_quat_target_.w()<<" "<<arm_end_quat_target_.x()<<" "<<arm_end_quat_target_.y()<<" "<<arm_end_quat_target_.z()<<" ";
#endif
        TypeTransfer<SocketDataType>::Type2ByteArray(buffer+index,static_cast<SocketDataType>(arm_end_quat_target_.w()));
        index+=sizeof (SocketDataType);
        TypeTransfer<SocketDataType>::Type2ByteArray(buffer+index,static_cast<SocketDataType>(arm_end_quat_target_.x()));
        index+=sizeof (SocketDataType);
        TypeTransfer<SocketDataType>::Type2ByteArray(buffer+index,static_cast<SocketDataType>(arm_end_quat_target_.y()));
        index+=sizeof (SocketDataType);
        TypeTransfer<SocketDataType>::Type2ByteArray(buffer+index,static_cast<SocketDataType>(arm_end_quat_target_.z()));
        index+=sizeof (SocketDataType);
    }
    if (updated_attr_&Master2Slave::kEndLinearVel)
    {
#ifdef PRINT_LOG
        cout<<" target_end_v: ";
#endif
        for (int i=0;i<3;i++) {
            TypeTransfer<SocketDataType>::Type2ByteArray(buffer+index,static_cast<SocketDataType>(arm_end_v_target_[i]));
            index+=sizeof (SocketDataType);
#ifdef PRINT_LOG
            cout<<arm_end_v_target_[i]<<" ";
#endif
        }
    }
    if (updated_attr_&Master2Slave::kEndAngularVel)
    {

#ifdef PRINT_LOG
        cout<<" target_end_w: ";
#endif
        for (int i=0;i<3;i++) {
            TypeTransfer<SocketDataType>::Type2ByteArray(buffer+index,static_cast<SocketDataType>(arm_end_w_target_[i]));
            index+=sizeof (SocketDataType);

#ifdef PRINT_LOG
            cout<<arm_end_w_target_[i]<<" ";
#endif
        }
    }
    if (updated_attr_&Master2Slave::kGripperPos)
    {
        TypeTransfer<SocketDataType>::Type2ByteArray(buffer+index,static_cast<SocketDataType>(finger_joint_pos_target_));
        index+=sizeof (SocketDataType);
#ifdef PRINT_LOG
        cout<<" target_gripper_p: "<<finger_joint_pos_target_;
#endif
    }
    if (updated_attr_&Master2Slave::kGripperVel)
    {
        TypeTransfer<SocketDataType>::Type2ByteArray(buffer+index,static_cast<SocketDataType>(finger_joint_vel_target_));
        index+=sizeof (SocketDataType);
#ifdef PRINT_LOG
        cout<<" target_gripper_v: "<<finger_joint_vel_target_;
#endif
    }
    if(updated_attr_&Master2Slave::kJointPos)
    {
#ifdef PRINT_LOG
        cout<<" target_joint_pos: ";
#endif
        for (int i=0;i<joint_num_;i++) {
            TypeTransfer<SocketDataType>::Type2ByteArray(buffer+sizeof (SocketDataType),static_cast<SocketDataType>(arm_joint_pos_target_[i]));
            index+=sizeof (SocketDataType);
#ifdef PRINT_LOG
            cout<<arm_joint_pos_target_[i]<<",";
#endif
        }
    }
    if(updated_attr_&Master2Slave::kJointVel)
    {
#ifdef PRINT_LOG
                cout<<" target_joint_vel: ";
#endif
        for (int i=0;i<joint_num_;i++)
        {
            TypeTransfer<SocketDataType>::Type2ByteArray(buffer+sizeof (SocketDataType),static_cast<SocketDataType>(arm_joint_vel_target_[i]));
            index+=sizeof (SocketDataType);
#ifdef PRINT_LOG
            cout<<arm_joint_vel_target_[i]<<",";
#endif
        }
    }
#ifdef PRINT_LOG
    cout<<endl;
#endif

    ClearTargetStatus();

    if(index<=2)
    {
        len=0;
        return false;
    }
    else
    {
//        cout<<"send msg:";
//        for (int i=0;i<index;i++)
//        {
//            cout<<(int)buffer[i]<<" ";
//        }
//        cout<<endl;
        len=index;
        return true;
    }
    //return index;
}

//bool ArmControllerImpl::LoadConfigFile(const char *path)
//{
//    if(path==nullptr)
//    {
//        //Eigen::Isometry3d mapping_m2s_;
//        //arm_end_origin_pos_ = Vector3d(0.4,-0.25,-0.4);
//        //arm_end_default_origin_pos_=arm_end_origin_pos_;
//        //mapping_ori_m2s_=Eigen::AngleAxisd(M_PI_2,Eigen::Vector3d::UnitY())*Eigen::AngleAxisd(-M_PI_2, Eigen::Vector3d::UnitX());
//        //mapping_coeff_m2s_=20;
//    }
//    return true;
//}

